from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, TimerAction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from moveit_configs_utils import MoveItConfigsBuilder


def generate_launch_description():
    package_path = get_package_share_directory("scepter_moveit_config")
    moveit_config = (MoveItConfigsBuilder("arm", package_name="scepter_moveit_config")
                     .robot_description('config/gz.arm.urdf.xacro')
                     .robot_description_semantic('config/arm.srdf')
                     .planning_pipelines(pipelines=["ompl", "chomp", "pilz_industrial_motion_planner", "stomp"])
                     .trajectory_execution(file_path="config/moveit_controllers.yaml")
                     .planning_scene_monitor(
                        publish_robot_description=True,
                        publish_robot_description_semantic=True
                    )
                     .to_moveit_configs())
    move_group_capabilities = {"capabilities": "move_group/ExecuteTaskSolutionCapability"}
    # 启动Gazebo
    gazebo_node = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            get_package_share_directory('ros_gz_sim')+'/launch/gz_sim.launch.py']),
            launch_arguments=[('gz_args','empty.sdf -r --physics-engine gz-physics-bullet-featherstone-plugin')]
    )

    #　将机械臂添加到Gazebo
    robot_to_gazebo_node = Node(
            package='ros_gz_sim',
            executable='create',
            arguments=[
                '-string',
                moveit_config.robot_description['robot_description'],
                '-x', '0.0', '-y', '0.0', '-z','0.0' ,'-name', 'arm'
            ]
        )

    # Clock Bridge
    clock_bridge_node = Node(
        package='ros_gz_bridge',
        executable='parameter_bridge',
        arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
        output='screen'
    )

    # 发布机器人各关节的状态信息
    # 根据机器人的URDF描述和关节状态，计算并发布机器人各连杆的变换关系(TF)
    # 为RViz、MoveIt等工具提供机器人当前状态的可视化数据
    # 设置使用仿真时间(use_sim_time)
    # 以同步Gazebo仿真时间
    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher",
        output="both",
        parameters=[
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            {'use_sim_time': True},
            {"publish_frequency": 30.0}
        ]
    )

    # 发布静态坐标变换
    # 建立world坐标系到机器人base_link坐标系的固定变换关系
    # 这对于将机器人固定在仿真环境中的特定位置非常重要
    static_tf_node = Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        name="static_transform_publisher",
        output="log",
        arguments=["--frame-id", "world", "--child-frame-id", "base_link"],
    )

    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        output="log",
        arguments=["-d", package_path + '/config/moveit.rviz'],
        parameters=[
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.robot_description_kinematics,
            moveit_config.planning_pipelines,
            moveit_config.joint_limits,
            {'use_sim_time': True},
        ]
    )

    # 启动控制器管理器的spawner工具
    # 加载并启动三种控制器：
    # joint_state_broadcaster - 关节状态广播器，发布所有关节的状态信息
    # arm_group_controller - 机械臂关节轨迹控制器，用于控制机械臂运动
    # gripper_group_controller - 夹爪控制器，用于控制夹爪开合
    # 替换原来的 controller_spawner_node
    joint_state_broadcaster_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
    )

    arm_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["arm_group_controller", "--controller-manager", "/controller_manager"],
    )

    gripper_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["gripper_group_controller", "--controller-manager", "/controller_manager"],
    )

    # 提供高层次的运动规划接口
    # 整合各种规划算法、运动学求解器、碰撞检测等
    # 为用户应用提供规划服务，如路径规划、姿态规划等
    move_group_node = Node(
        package="moveit_ros_move_group",
        executable="move_group",
        output="screen",
        parameters=[
            moveit_config.to_dict(),
            move_group_capabilities,
            {'use_sim_time': True}
        ]
    )
    return LaunchDescription([
            robot_state_publisher,
            static_tf_node,
            TimerAction(period=3.0, actions=[rviz_node]),
            TimerAction(period=5.0, actions=[joint_state_broadcaster_spawner]),
            TimerAction(period=7.0, actions=[arm_controller_spawner]),
            TimerAction(period=9.0, actions=[gripper_controller_spawner]),
            TimerAction(period=5.0, actions=[move_group_node]),
            TimerAction(period=6.0, actions=[gazebo_node]),
            TimerAction(period=7.0, actions=[robot_to_gazebo_node]),
            TimerAction(period=8.0, actions=[clock_bridge_node])
        ])







